#ifndef MESSAGE_TYPES_H
#define MESSAGE_TYPES_H
//!@author James Hudack
//!@author Seth Sims

/*!@brief This packet tells us how to treat the data that follows it.
 *
 * @var tv_sec The time Rotomotion autopilot thinks it is in seconds
 * @var tv_usec The time Rotomotion autopilot thinks it is in microseconds (sec*10^-6)
 * @var type Descriminates between the different types of packets send by Rotomotion Autopilot
 *
 *  The type variable is defined by Rotomotion Autopilot and tells us which packet type follows.
 *  Most Rotomotion packets use this basic format.
 */
typedef struct MSG_ROTOMOTION_BASE {
    uint32_t tv_sec, tv_usec;
    uint32_t type;
} msg_base;

/*!@brief (Message type: 10) A basic status packet that relates the current state of the Rotomotion Autopilot
 *
 * @var base The packet discriminator that tells us how to manage the packet
 * @var phi Euler angle relative to the NED tangent plane in radians
 * @var theta Euler angle relative to the NED tangent plane in radians
 * @var psi Euler angle relative to the NED tangent plane in radians
 * @var north The NED north coordinate of the helecopter relative to the reference point (kalman filtered)
 * @var east The NED east coordinate of the helecopter relative to the reference point (kalman filtered)
 * @var down The NED down coordinate of the helecopter based on the WGS-84 ellipsoid (kalman filtered)
 * @var vnorth The velocity of the helecopter in the northward direction
 * @var veast The velocity of the helecopter in the eastward direction
 * @var voltage IMU bus voltage
 * @var resets The number of IMU resets
 *
 * The main status packet sent by Rotomotion Autopilot. Relates nearly all information about the helicopter
 * in one big chunk.
 */
typedef struct MSG_STATE {
    msg_rotomotion_base base;  
    double      phi, theta, psi;  // Euler angles relative to the ground (radians) 
    double      p, q, r;			// Body frame rotational rates rad/s
    double      north, east, down;  // filtered Position relative to the local tangent plane
    double      v_north,v_east,v_down;			// Velocity over the ground m/s  
    double      mx, my, mz;		// Raw magnetometer readings  m/s/s
    double      ax, ay, az;		// Body frame linear accelerations 
    double      raw_p, raw_q, raw_r;    // Raw body rates 
    double      ahrs_trace;			//ahrs trace :  0>t<10 = good	
    double 	  voltage;			//IMU bus voltage
    uint16_t  resets;			//# IMU resets
    uint16_t  rpm0, rpm1, rpm2;  //engine, main, tail rpm
} msg_state;

/*!@brief (Message type: 12) The message containing gps data
 *
 * @var base The packet discriminator that tells us how to manage the packet
 * @var numsv The number of gps satalites in view divided by SV
 * @var velocity_accuracy The accuracy of the velocity readings
 * @var positional_accuracy The accuracy of the positional readings
 * @var velocity_north The northward velocity of the helicopter
 * @var velocity_east The eastward velocity of the helicopter
 * @var velocity_down The downward velocity of the helicopter
 * @var position_north The NED north coordinate of the helicopter relative to the reference point
 * @var position_east The NED east coordinate of the helicopter relative to the reference point
 * @var position_down The NED down coordinate based on the WGS-84 ellipsoid
 * @var filter_quality The confidence rating of the kalman filter (Anything [0, 100] is ok) 
 *
 * A datapacket that contains the positional and velocity of the helicopter. It also contains
 * various other data relative to the gps.
 */
typedef struct MSG_GPS {					//ID=12, 
    msg_rotomotion_base base;
	short	numsv;						//# of sats/SV
	short	velocity_accuracy;
    short   positional_accuracy;				//speed accuracy m/s - vdop and pdop
	short	fixtype;
	double	velocity_north;
    double  velocity_east;
    double  velocity_down;	//ned velocity
	double	position_north;
    double  position_east;
    double  position_down;	//ned position	from tangent plane
	double  raw_pos_n, raw_pos_e, raw_pos_d;		//raw pos in ECEF format
	double	filter_quality;					//kalman filter quality :  0<t<100 = OK
} msg_gps;

//////////////////////////////////////////////////////////////////////////////////
// the SV info structure msg would be here if needed
///////////////////////////////////////////////////////////////////////////////////

typedef struct MSG_AUTOPILOT {		//ID=14 
    msg_rotomotion_base base;
    int16_t 		  manual_ap, mode_ap;	 
    uint16_t  roll_ap,pitch_ap,yaw_ap,throttle_ap,coll_ap,padding_ap;// current generated servo commands 
    double      n_d_ap,e_d_ap,d_d_ap;								// desired Position relative to the ground 
    double      vx_d_ap,vy_d_ap,vz_d_ap;								// desired Velocity over the ground 
    double      phi_d_ap,theta_d_ap,psi_d_ap;						// desired Euler angles relative to the ground 
    double      p_d_ap, r_d_ap, q_d_ap;								// des Body frame rotational rates 
} msg_autopilot;

//!@brief (Message type: 15) Some kind of status packet. I don't know exactly what it does.
struct MSG_AFCS {					//ID=15/ afcs
	msg_rotomotion_base base;
    uint16_t	gps_status;		//0=no sig, 1=<3 SVs, 2=<7 SVs, 3= >7 SVs, low dop
	uint16_t	radio_status;	//QOS level:  0= <100, 1=<150, 2=<200, 3=>500
	uint16_t	ahrs_status;	//?
	uint16_t	takeoff_status;	//?
	uint16_t	autopilot_status;    //?
	uint16_t	unused_state;		// unused
} msg_afcs;

/*!@brief (Message type: 17) Sets a new position for the NED tangent plane reference point
 *
 * @var base The packet discriminator that tells us how to manage the packet
 * @var latitude The latitude of the NED reference point in radians
 * @var longitude The longitude of the NED reference point in radians
 * @var height The height of the NED reference point in meters
 *
 * Sets a new NED reference point and redefines the tangent plane used by the autopilot.
 * Somehow it can be sent in a way that the current position of the helecopter becomes
 * the NED reference point.
 */
struct MSG_TANGENT_PLANE {			//ID=17  send with NO arguement if resetting to zero
	msg_rotomotion_base base;
    double latitude;
    double longitude;
    double height;		// LLH (radians, radians, meters)
} msg_tangent_plane;

/*!@brief (Message type: 16) This message is sent to command Rotomotion Autopilot to seek to a new position.
 *
 * @var base The packet discriminator that tells us how to manage the packet
 * @var position_mode I don't know
 * @var heading_mode I don't know
 * @var transit_time If positive this tells the autopilot the number of seconds you wish it to make
 *                   the trip in. If negitive it is a desired velocity in cm/s.
 * @var pad1 unused
 * @var north The NED north coordinate to seek to
 * @var east The NED east coordinate to seek to
 * @var down The NED down coordinate to seek to
 * @var heading Depending on the heading mode, this is either the desired heading relative to north
 *              or an offset from the current heading. It is in CCW radians.
 */
typedef struct MSG_FLYTO {				//ID=16  - SEND ONLY
	msg_rotomotion_base base;
    int16_t			position_mode;
    int16_t         heading_mode;
	int16_t			transit_time;		//transit time in s or vel in cm/s if negative	 
	uint16_t    	pad1;               //unused padding 
	double          north;
    double          east;
    double          down;               //Position relative to the ground ned in meters
	double          heading;            // Heading or offset in radians
} msg_flyto;

typedef struct MSG_VELSCALE { //id = 23
	msg_rotomotion_base base;
    double	vel_scale;						// k/(v+k)  //where v= speed in m/s
} msg_velscale;

/*!@brief (Message type: 20) This is sent from Rotomotion Autopilot as a responce to a flyto packet. 
 * It lists the position the autopilot is trying to navagate to.
 *
 * @var base The packet discriminator that tells us how to managet the packet
 * @var desired_north The NED north coordinate of the position being flown to
 * @var desired_east The NED east coordinate of the position being flown to
 * @var desired_down The NED down coordinate of the position being flown to
 * @var heading The absolute heading CCW relative to North in radians
 *
 * This is sent as conformation from the autopilot as a responce to a flyto packet. (Message type 20)
 */
typedef struct MSG_DESIRED_POS {
    msg_rotomotion_base base;
    double desired_north;
    double desired_east;
    double desired_down;
	double heading;
} msg_desired_pos;

typedef struct MSG_PPM  {			//ID= 11 --- partially in use
    msg_rotomotion_base base;
    uint16_t     valid;    // channel valid status bit mask
    uint16_t     roll_ppm, pitch_ppm, throttle_ppm, yaw_ppm, yaw_gain_ppm, coll_ppm;// servo ( channel 0-5 )
    uint16_t     ppm_mode;     // raw flight mode  ( channel 6 )
    uint16_t     extra1, extra2; // unused value + Knob ( channel 7 ), respectively
    uint16_t     auto_manual;   // auto/manual mode  0 is auto / 1 is manual
    uint16_t     idle;     // Psudeo-channel that is 0 when the throttle is below the first point on the throttle curve
    int16_t      gyro_gain; // Scaled gyro gain from -100 to +100
    uint16_t     crc_err;  // Number of PCM CRC errors
    uint16_t     glitches; // Quality of Radio Link Signal 255 best, less not better
    uint16_t     mask;     // Mask of which servos are manually controlled and which are auto
} msg_ppm;

// GAINs for RM autopilot //////////////////////////////////////////////////////////
typedef struct MSG_X_GAIN {			//id=56
	double		x_gain_p, x_gain_i, x_gain_d;
	double		x_min_p_limit,x_max_p_limit;
	double		x_min_i_limit,x_max_i_limit;
	double		x_min_d_limit,x_max_d_limit;
	double		x_min_outp, x_max_outp;
} msg_x_gain;							
							
struct MSG_Y_GAIN {			//id=57
	double		y_gain_p, y_gain_i, y_gain_d;
	double		y_min_p_limit,y_max_p_limit;
	double		y_min_i_limit,y_max_i_limit;
	double		y_min_d_limit,y_max_d_limit;
	double		y_min_outp, y_max_outp;
} msg_y_gain;							

struct MSG_Z_GAIN {			//id=58
	double		z_gain_p, z_gain_i, z_gain_d;
	double		z_min_p_limit,z_max_p_limit;
	double		z_min_i_limit,z_max_i_limit;
	double		z_min_d_limit,z_max_d_limit;
	double		z_min_outp, z_max_outp;
} msg_z_gain;							

//!@brief New , but don't use so says James. I dont know what this does.
struct MSG_AFCS_MODE {		
	uint32_t   tv_sec, tv_usec, type;    //id = 18
	uint16_t    mask;  //bit mask for auto controls
} msg_afcs_mode;
	
struct MSG_TELEOP {
	uint32_t        tv_sec, tv_usec, type;
	double			fore, side, alt, hdg;  //velocity commands from base joystick
} msg_teleop;

struct MSG_FAILSAFE {
	uint32_t    tv_sec, tv_usec, type;
	uint16_t	pos_mode, hdg_mode, transit_time, timeout;  //see RM docs for codes
	double x,y,z,hdg;   //desired failsafe point
} msg_failsafe;

struct MSG_MAGCAL {
	uint32_t    tv_sec, tv_usec, type;
	int16_t     x_index, my_index, mz_index, smoothing;
	int16_t     mx_hardiron, my_hardiron, mz_hardiron, pad0;
	double		mx_scale, my_scale, mz_scale;
} msg_magcal;

// fadec should go here, but it won't be used unless the hardware is updated for it.
//
#endif
